
#include "main.hpp"


 uint64_t time_5ms = 0;
void* control_thread(void* arg)
{
    uint8_t loop_1Hz  = 0;
    uint8_t loop_5Hz  = 0;
    uint8_t loop_10Hz = 0;
    uint8_t loop_20Hz = 0;
    uint8_t loop_50Hz = 0;
    uint8_t loop_100Hz = 0;


    usr_thread_t *temp = (usr_thread_t*)arg;
    uint8_t radio_rcv_buf[128];
    uint8_t SBUS_rcv_buf[128];
     uint8_t landpad_RTK_rcv_buf[128];

    while(1)
    {
        //  NX_serial_read_raw_data(temp->usr_radio, &groundbase_ringbuf);
         UDP_read_raw_data(UDP_fd,&groundbase_ringbuf);
         NX_analyse_groundbase_data(&groundbase_ringbuf, radio_rcv_buf);

        if((micros()-time_5ms)>=5000){
            time_5ms = micros();
            loop_100Hz++;
            loop_20Hz++;
            loop_50Hz++;
            loop_10Hz++;
            loop_5Hz++;
            loop_1Hz++;	
        }
        if(loop_100Hz>=2){
            loop_100Hz = 0;
        }
        if(loop_50Hz>=4){
            loop_50Hz = 0;

        }
        if(loop_20Hz>=10){
            loop_20Hz = 0;
          if(if_up_and_down_waypoint_falg == false)
          {
            NX_serial_send_to_groundbase_20hz(temp->usr_radio,&drone_data_to_groundbase);
          
          }
                   
        }
        if(loop_10Hz>=20){
            loop_10Hz = 0;
            NX_serial_send_to_groundbase_of_waypoint_ACK(temp->usr_radio);
            if(if_up_and_down_waypoint_falg == false)
            {
                NX_serial_send_to_groundbase_10hz(temp->usr_radio,&drone_data_to_groundbase);
            }
           
            get_TF_mini_data(); 
        }
        if(loop_5Hz>=40){
    
            loop_5Hz = 0;
 
        }

        if(loop_1Hz>=200){
            loop_1Hz = 0;

            if(if_up_and_down_waypoint_falg == true)
            {
                NX_get_up_down_waypoint_cnt++;
            }
   
            usr_flag.NX_wait_groundbase_controlcmd_ready_flag++;
            usr_flag.gcs_lose_heart_beat_flag = true;
             usr_flag.NX_get_groundbase_heartbeat_cnt++;

            if(usr_flag.NX_wait_groundbase_controlcmd_ready_flag >= 3)
            {
                usr_flag.NX_wait_groundbase_controlcmd_ready_flag = 3;
            }

           std::cout << " 我是地面站线程：" << std::endl;
           printf("UAV_acc = %d\n",remote_cmd.UAV_acc);
           printf("groundbase_cmd = %d\n",groundbase_cmd.control_cmd);
            printf("111111111111111111111111111111111111111111111111111高度 = %d\n",drone_data_to_groundbase.curr_pos.alt);

        }
        
    }
    
}



//
void* SBUS_thread(void* arg)
{
    usr_thread_t *temp = (usr_thread_t*)arg;
    uint8_t SBUS_rcv_buf[128];
    uint8_t loop_1Hz  = 0;
    while(1)
    {
            if((micros()-time_5ms)>=5000){
            time_5ms = micros();
            loop_1Hz++;	
        }
         NX_serial_read_SBUS_raw_data(temp -> usr_SBUS,&SBUS_ringbuf);
         NX_analyse_SBUS_data(&SBUS_ringbuf,SBUS_rcv_buf);

        if(loop_1Hz>=200){
             loop_1Hz = 0;
                std::cout << " 我是SBUS线程：" << std::endl;
        }
    }

}
